//
// Created by dwx on 2024/1/22.
//

#ifndef SIMPLEFOC_BSP_FOCUTILS_H
#define SIMPLEFOC_BSP_FOCUTILS_H
#ifdef __cplusplus
extern "C" {
#endif


#include "math.h"

#define _sign(a) (((a)<0)?-1:((a)>0))
#define _round(x)  ((x)>=0?(long)((x)+0.5f):(long)((x)-0.5f))
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
#define _sqrt(a) (_sqrtApprox(a))
#define _isset(a) ((a)!=(NO_SET))


#define _2_SQRT3 1.15470053838f
#define _SQRT3   1.73205080757f
#define _1_sqrt3 0.57735026919f
#define _SQRT3_2 0.86602540378f
#define _SQRT2   1.41421356237f
#define _120_D2R 2.09439510239f
#define _PI      3.14159265359f
#define _PI_2    1.57079632679f
#define _PI_3    1.0471975512f
#define _2PI     6.28318530718f
#define _3PI_2   4.71238898038f
#define _PI_6    0.52359877559f

typedef struct{
    float d;
    float q;
}DQCurrent_s;
typedef struct{
    float a;
    float b;
    float c;
}PhaseCurrent_s;
typedef struct{
    float d;
    float q;
}DQVoltage_s;

float _sin(float a);
float _cos(float a);
float _normalizeAngle(float angle);
float _electricalAngle(float shaft_angle, int pole_pairs);
float _sqrtApprox(float number);


#ifdef __cplusplus
}
#endif

#endif //SIMPLEFOC_BSP_FOCUTILS_H
